//#pragma once 

#ifndef HYBRID_GRAPH_H
#define HYBRID_GRAPH_H

#include "Graph\Graph.h"
#include "CollisionDetector.h"

//#include <iostream>

//namespace hGraph{

// Reference_Point should have location and rotation
template<typename K>
class HNode
{
public:
	typedef Reference_point<K>		ReferencePoint;

private:
	ReferencePoint m_point;

public:
	HNode() : m_point(){}

	HNode(ReferencePoint const & point) : m_point(point){}

	HNode& operator=(HNode const & other)
	{
		m_point = other.m_point;
		return *this;
	}

	//compute the distance squared to other
	double dist2(HNode const & other) const
	{
		return std::sqrt(std::pow(other.x()-x(),2)+std::pow(other.y()-y(),2));
	}

	//compute the norm squared
	double norm2() const{
		return std::sqrt(x()*x()+y()*y());
	}

	// the closer to the origin the smaller the node is 
	bool operator<(HNode const & other) const
	{
		if (m_point.get_location() != other.m_point.get_location()){
			return norm2() < other.norm2();
		}else{
			return std::abs(m_point.get_rotation().to_angle()) < 
				std::abs(other.m_point.get_rotation().to_angle());
		}
	}

	bool operator==(HNode const & other) const
	{
		return m_point == other.m_point;
	}

	ReferencePoint ref_point() const
	{
		return m_point;
	}

	double x() const
	{
		return CGAL::to_double(m_point.get_location().x());
	}

	double y() const
	{
		return CGAL::to_double(m_point.get_location().y());
	}

	double angle() const
	{
		return m_point.get_rotation().to_angle();
	}
};

template<typename K>
ostream& operator<<(ostream& os,HNode<K> const& node)
{
	return os << node.ref_point();
}  


template <typename K> //K is the kernel type
class H_Graph
{

public:
	/*
	typedef CGAL::Polygon_2<K>				Polygon;
	typedef std::vector<Polygon>			Polygon_vector;
	typedef Extended_polygon<K>				Extended_polygon;
	typedef CGAL::Point_2<K>				Point2D;
	typedef HNode<Point2D>					Node;
	typedef std::vector<Point2D>			Path;
	*/
	typedef typename K::FT						NT; //number type
	typedef CGAL::Polygon_2<K>					Polygon;
	typedef std::vector<Polygon>				Polygon_vector;
	typedef Reference_point<K>					Reference_point;
	typedef std::vector<Reference_point>		Path;
	typedef HNode<K>							Node;


private:

	Node m_src;

	Node m_trg;

	NT m_threshold;
	
	Polygon const m_robot;

	Graph<Node>* m_graph;

	CollisionDetector<K> m_detector;

	std::vector<Path> m_paths;


protected:

	void naive_merge(Path new_path)
	{
		for (unsigned int i = 0; i < new_path.size()-1; i++){
			Node s = new_path[i];
			for (unsigned int k = 0; k < m_paths.size(); k++){
				Path old_path = m_paths[k];
				for (unsigned int j = 1; j < old_path.size(); j++){
					Node t = old_path[j];
					if (s.dist2(t) > m_threshold || s.dist2(t) == 0) continue;
					Path extra;
					if(m_detector.isValid(m_robot,s.ref_point(),t.ref_point(),extra)){
						///
						std::cout << "s: " <<  s  << " t: " <<  t << std::endl;
						global_graphics->draw_edge<K>(s.ref_point().get_location(),t.ref_point().get_location(),GREEN);
						///
						Node v1(extra[0]); Node v2(extra[1]);
						m_graph->add_vertex(v1);
						m_graph->add_vertex(v2);

						m_graph->add_edge(s,v1,0.0001);
						m_graph->add_edge(v1,v2,v1.dist2(v2));
						m_graph->add_edge(v2,t,0.0001);
					}
				}
			}
		}
	}


	void init(Path const & path)
	{
		init(path[0],path[path.size()-1])
		add_path(path);
	}

	void init(Reference_point const & s,Reference_point const & t)
	{
		m_src = s;
		m_trg = t;
		m_graph->add_vertex(s);
		m_graph->add_vertex(t);
	}


public:

	H_Graph(Polygon_vector const & workspace,Polygon const & robot,
		Reference_point const & s,Reference_point const & t, NT D=NT(0.25)) : m_robot (robot),
		  m_detector(workspace),
		  m_src(s),
		  m_trg(t),
		  m_threshold(D)
	{
		m_graph = new Graph<Node>();
		init(s,t);
	}
	
	H_Graph(Polygon_vector const & workspace,Polygon const & robot, Path const & path,NT D=NT(0.25)) : 
		m_robot (robot),
		m_detector(workspace),
		m_threshold(D)
	{
		m_graph = new Graph<Node>(path.size());
		init(path);
	}

	void add_path (Path const & path)
	{
		if (path.size() < 2) return;
		
		for (unsigned int i = 1 ; i < path.size()-1; i++){

			m_graph->add_vertex(path[i]);
		}
		for (unsigned int i = 1 ; i < path.size(); i++){
			Node prev = path[i-1];
			Node next = path[i];
			double w = next.dist2(prev)+0.0001;
			m_graph->add_edge(prev,next,w);
		}
		naive_merge(path);

		m_paths.push_back(path);
	}


	void add_obstacle(Polygon const & polygon)
	{
		m_detector.add_obstacle(polygon);
	}

	Path compute_optimal_path()
	{
		Path path;
		std::list<Node> nodes;
		m_graph->find_path(m_src,m_trg,nodes);

		for (std::list<Node>::iterator it = nodes.begin() ;it != nodes.end();it++){
			path.push_back(it->ref_point());
		}
		return path;
	}

	virtual ~H_Graph()
	{
		delete m_graph;
	}
};


//}
#endif //HYBRID_GRAPH_H